package dev;

/**
 * ROBIN WILLIS INFARED 3D VIEWER
 * 
 * thanks to Classiclll
 * 
 *  T - toggle variable checking
 *  D - toggle debug
 *  S - toggle sensor test
 **/

import processing.core.*;
import lll.wrj4P5.*;
import lll.Loc.*;
import peasy.*;
import java.util.*;
import controlP5.*;
import processing.opengl.*;
import javax.media.opengl.*;
import mri.*;
import saito.objloader.*;

public class Robo_Sketch extends PApplet {
	private static final long serialVersionUID = 1L;
	
	//declare classes
	Wrj4P5 wii;
	public IrPen4P5 ir;
	Space_Veiwer cam;
	PFont Arial;
	OBJModel model;
	
	//Point Holders for Display
	HashSet spots = new LinkedHashSet();
	HashSet spots2 = new LinkedHashSet();
	
	//Point Holders for Navigation
	ArrayList pList1;
	ArrayList pList2;
	
	//Points for Calculations
	public static Loc[] point1 = {new Loc(),new Loc()};
	public static Loc[] point2 = {new Loc(),new Loc()};
	public Loc[] point3 = {new Loc(),new Loc()};
	Loc mP1, mP2;
	
	//Variables
	boolean varText = false;
	boolean irSensorTest = false;
	boolean drawSpace = true;
	boolean usingWiimote = false;
	int lineHeight = 18;
	int textStart = 100;
	
	float  p1dx, p1dy, p2dx, p2dy, p3dx, p3dy; 
	float  p1delta, p2delta, p3delta;
	float  p1Dist, p2Dist, p3Dist;
	float curDist, prevDist, compDist;
	double pAngle;
  boolean checkP1Dist, checkP2Dist, checkP3Dist;
  
  
  int initBoidNum = 200; //amount of boids to start the program with
  BoidList flock1;//,flock2,flock3;
  
  float zoom=800,rX=0,rY=0; //camera variables
  boolean smoothEdges = false,avoidWalls = false,lighting = false,movingCamera=false; //some booleans for user settings
  int spaceCounter = 0; //counter variables

	public void setup() {
	  size(640,480,P3D);
	  frameRate(50);
	  
	  //seup opengl  
	  hint( ENABLE_OPENGL_4X_SMOOTH );
	  smooth();

	  //setup wii

	    wii = new Wrj4P5(this);
	    ir =  new IrPen4P5(this, wii.rimokon,false);
	 if(usingWiimote){
	    wii.connect();
	  }
	  //setup camera
	  cam = new Space_Veiwer(this,500);
	  cam.setMinimumDistance(1);
	  cam.setMaximumDistance(5000);
	  cam.setMouseControlled(false);
	  

	  //setup font
	  Arial = createFont("Arial",12);
	  textFont(Arial);
	  
	  pList1 = new ArrayList();
	  pList2 = new ArrayList();
	  
	  flock1 = new BoidList(this, initBoidNum,255);
	  

	} 

	public void draw() {
		 
	  background(10);
	  cam.beginHUD();
	  translate(0,0,-69);  //640x680
   // translate(0,0,-175); // 800x600 bug - for some reason peasycam hud only works at 400x400px
   // translate(0,0,-319); // 1024x768

  if(usingWiimote){
  	  if (wii.isConnecting()){ //before anything connect wii
  	   text("wii is connecting",375,300);
  	   cam.endHUD();
  	   return; 
  	  }
  }
	  if(irSensorTest){ //turn on to check to make sure wiimote can read the ir lights
	    ir.sensorTest(); 
	    cam.endHUD();
	    return; 
	  }
	 
	if(usingWiimote){
	  if (ir.isCalibrating()){ //calibrate the pens
	  cam.endHUD();
	 return; 
	  }
	}  
//START NAVIGATION
	  

	  if(usingWiimote){
	  //get pen values
	    //************ IR INTERACTION (use with wiimote)
	     getPenValues();
	    //************ IR INTERACTION (use with wiimote)   
	  }else{
	    //************ IR EMULATION (use with mouse)
	    irEmulation();
	   //************ END IR EMULATION (use with mouse)
	  }
	 
	 
	  if (varText){ 
		  checkVariables();//load up text to display variables at work
		  drawPoints();
	  }

	 cam.endHUD();
		
	 // Move Camera
	 
	 if (ir.npen() == 1){
		 cam.penRotate(p1dx/7,p1dy/7,point1[1].x,point1[1].y);
	 }
	 else if (ir.npen()>1){
	 
	 	if(abs(p3dx) > abs(compDist) || abs(p3dy) > abs(compDist)){
	 	 	
	 	 	cam.penPan(p3dx/3,p3dy/3);
	 	 	
	 	}else{
	 		cam.penZoom(-compDist/14);
			cam.penRotate(p2dy/7,p2dx/7,0,0);
			cam.penRotate(p1dx/7,p1dy/7,0,0);
	 	}
	
	 }
	 cam.drawStage();
	 	//Draw Stage 	
	 	if(drawSpace){ 
	 		 
	 	 	stroke(255);
		 	fill(200,50);
		 	//drawBounds();

		  flock1.run(false);
	 	}
	 
	}
	
	
	void getPenValues(){
		  
		//***************** ONE PEN *****************		    
		  if (ir.npen() == 1){
			  
			  noStroke();
			  fill(0,50,0);
			  ellipse(10,10,10,10);
			  
			//get first pen

			  spots.add(ir.pen(0));
			  pList1.add(ir.pen(0));
		   
		      if(pList1.size()>2 && pList1.get(pList1.size()-2) != null){
		    	  
		    	  	Loc ppr = (Loc) pList1.get(pList1.size()-2); 
			   		point1[0].x = (ppr.x*width);
			   		point1[0].y = (ppr.y*height);
			   }
		      
			   if(pList1.size()>1 && pList1.get(pList1.size()-1) != null){
				   
				   	Loc pr =  (Loc) pList1.get(pList1.size()-1);
			   		point1[1].x = (pr.x*width);
			   		point1[1].y = (pr.y*height);
			   }
		   
			   //update values
			   
			   updatePoints();
			   
		  }
		  
		//***************** TWO PENS *****************
		  
		  else if(ir.npen()>1){
			  
			  noStroke();
			  fill(50,0,0);
			  ellipse(width-10,10,10,10);
			  //get first pen

			  spots.add(ir.pen(0));
			  pList1.add(ir.pen(0));
		   
		      if(pList1.size()>2 && pList1.get(pList1.size()-2) != null){
		    	  
		    	  	Loc ppr = (Loc) pList1.get(pList1.size()-2); 
			   		point1[0].x = (ppr.x*width);
			   		point1[0].y = (ppr.y*height);
			   }
		      
			   if(pList1.size()>1 && pList1.get(pList1.size()-1) != null){
				   
				   	Loc pr =  (Loc) pList1.get(pList1.size()-1);
			   		point1[1].x = (pr.x*width);
			   		point1[1].y = (pr.y*height);
			   }
		
			   //get second pen

			  spots2.add(ir.pen(1));
			  pList2.add(ir.pen(1));
			  
			  if(pList2.size()>2 && pList2.get(pList2.size()-2) != null){
				  
				  	Loc ppr2 = (Loc) pList2.get(pList2.size()-2); 
				  	point2[0].x = (ppr2.x*width);
				  	point2[0].y = (ppr2.y*height);
			   }
			   
			  if(pList2.size()>1 && pList2.get(pList2.size()-1) != null){
				  
				  Loc pr2 =  (Loc) pList2.get(pList2.size()-1);
				  point2[1].x = (pr2.x*width);
				  point2[1].y = (pr2.y*height);
			   }
		
			  //update values
			  
			  updatePoints();
		  }   
		  
		  //***************** NO PENS *****************
		  
		  else{ //Stop moving object if no pens pressed
		       	   
			  if(pList1.size()>1 && pList1.get(pList1.size()-1) != null){
		    	   Loc pr =  (Loc) pList1.get(pList1.size()-1);
		    	   point1[0].x = (pr.x*width);
		    	   point1[0].y = (pr.y*height);
		    	   point1[1].x = (pr.x*width);
		    	   point1[1].y = (pr.y*height);
		       }
				   
			  if(pList2.size()>1 && pList2.get(pList2.size()-1) != null){
				   Loc pr2 =  (Loc) pList2.get(pList2.size()-1);
				   point2[0].x = (pr2.x*width);
				   point2[0].y = (pr2.y*height);
				   point2[1].x = (pr2.x*width);
				   point2[1].y = (pr2.y*height);
			   }
				  
			  updatePoints();		  
		  }
	}

	void irEmulation(){
		
	  if(mousePressed && mouseButton == LEFT){
		 
		  wii.rimokon.irLights[0].move((float)(mouseX)/width,(float)(mouseY)/height,-1);
		  wii.rimokon.irLights[1].move((float)(mouseX)/width,(float)(mouseY)/height,-1);
		 
		  Loc mp1 =  new Loc ((wii.rimokon.irLights[0].x), (wii.rimokon.irLights[0].y),0);
	      
		  Loc mp2 =  new Loc ((wii.rimokon.irLights[1].x), (wii.rimokon.irLights[1].y),0);
		   
		  
		  pList1.add(mp1);
	      spots.add(mp1);
	     
	      
			  
	      
	      if(pList1.size()>2 && pList1.get(pList1.size()-2) != null){
	    	  
	    	  	Loc ppr = (Loc) pList1.get(pList1.size()-2); 
		   		point1[0].x = (ppr.x*width);
		   		point1[0].y = (ppr.y*height);
		   }
	      
		   if(pList1.size()>1 && pList1.get(pList1.size()-1) != null){
			   
			   	Loc pr =  (Loc) pList1.get(pList1.size()-1);
		   		point1[1].x = (pr.x*width);
		   		point1[1].y = (pr.y*height);
		   }
	   
		   pList2.add(mp2);
		   spots2.add(mp2);
		   
		   if(pList2.size()>2 && pList2.get(pList2.size()-2) != null){
				  
			  	Loc ppr2 = (Loc) pList2.get(pList2.size()-2); 
			  	point2[0].x = (width-ppr2.x*width);
			  	point2[0].y = (height-ppr2.y*height);
		   }
		   
		  if(pList2.size()>1 && pList2.get(pList2.size()-1) != null){
			  
			  Loc pr2 =  (Loc) pList2.get(pList2.size()-1);
			  point2[1].x = (width-pr2.x*width);
			  point2[1].y = (height-pr2.y*height);
		   }
		  
		  //update values
		   
		   updatePoints();
	      
	  
		   
		   
	  }else if (mousePressed && mouseButton == RIGHT){
	    
		  wii.rimokon.irLights[0].move((float)(mouseX)/width,(float)(mouseY)/height,-1);
	      wii.rimokon.irLights[1].move((float)(mouseX)/width,(float)(mouseY)/height,-1);
	    
	      
		  Loc mp1 =  new Loc ((wii.rimokon.irLights[0].x), (wii.rimokon.irLights[0].y),0);
	      
		  Loc mp2 =  new Loc ((wii.rimokon.irLights[1].x), (wii.rimokon.irLights[1].y),0);
		   
		  
		  pList1.add(mp1);
	      spots.add(mp1);
	     
	      
			  
	      
	      if(pList1.size()>2 && pList1.get(pList1.size()-2) != null){
	    	  
	    	  	Loc ppr = (Loc) pList1.get(pList1.size()-2); 
		   		point1[0].x = (ppr.x*width);
		   		point1[0].y = (ppr.y*height);
		   }
	      
		   if(pList1.size()>1 && pList1.get(pList1.size()-1) != null){
			   
			   	Loc pr =  (Loc) pList1.get(pList1.size()-1);
		   		point1[1].x = (pr.x*width);
		   		point1[1].y = (pr.y*height);
		   }
	   
		   pList2.add(mp2);
		   spots2.add(mp2);
		   
		   if(pList2.size()>2 && pList2.get(pList2.size()-2) != null){
				  
			  	Loc ppr2 = (Loc) pList2.get(pList2.size()-2); 
			  	point2[0].x = (ppr2.x*width+10);
			  	point2[0].y = (ppr2.y*height+10);
		   }
		   
		  if(pList2.size()>1 && pList2.get(pList2.size()-1) != null){
			  
			  Loc pr2 =  (Loc) pList2.get(pList2.size()-1);
			  point2[1].x = (pr2.x*width+10);
			  point2[1].y = (pr2.y*height+10);
		   }
		  
		  //update values
		   
		   updatePoints();
	      
	      
	      
	     // point1[1]  =  new Loc ((wii.rimokon.irLights[0].x*width), (wii.rimokon.irLights[0].y*height),0);
	     // point2[1]  =  new Loc ((wii.rimokon.irLights[1].x *width + 100),  (wii.rimokon.irLights[1].y *height + 100),0);
	  
	  }else{
		  wii.rimokon.irLights[0].move(-1,-1,-1);
		  wii.rimokon.irLights[1].move(-1,-1,-1);
	  }
	}
	
	void updatePoints(){
     
	 if(point1[1].x != point1[0].x){
		fill(255);
		noStroke();
		//rect(10,10,300,50); 
	 }

		checkP1Dist = point1[1].isNearTo(point1[0], 50);
	    checkP2Dist = point2[1].isNearTo(point2[0], 50);
		checkP3Dist = point3[1].isNearTo(point3[0], 50);
	    
	    if (checkP1Dist && checkP2Dist){
	    //point 1 pPoint 1 dist 
	    p1dx = point1[1].x - point1[0].x;
	    p1dy = point1[1].y - point1[0].y;
	    
	    //point 2 pPoint 2 dist
	    p2dx = point2[1].x - point2[0].x;
	    p2dy = point2[1].y - point2[0].y;
	 
	   Vector3D vP1 = new Vector3D((double)point1[1].x,(double)point1[1].y,(double)0);
	   Vector3D vP2 = new Vector3D((double)point2[1].x,(double)point2[1].y,(double)0);

   	   Vector3D vPp1 = new Vector3D((double)point1[0].x,(double)point1[0].y,(double)0);
	   Vector3D vPp2 = new Vector3D((double)point2[0].x,(double)point2[0].y,(double)0);
	 
	   Vector3D vSub =  vP1.subtract(vP2);
	   Vector3D vPsub = vPp1.subtract(vPp2);
	  
	   double pAngle; 
	   pAngle = Vector3D.angle(vSub,vPsub);
	 
	   Vector3D vMid =  vP1.add(vP2);
	   vMid = vMid.scalarMultiply((double)0.5);
	    
	    point3[1].x = (float)vMid.getX();
	    point3[1].y = (float)vMid.getY();

	    Vector3D vPmid =  vPp1.add(vPp2);
	    vPmid = vPmid.scalarMultiply((double)0.5);

	    point3[0].x = (float)vPmid.getX();
	    point3[0].y = (float)vPmid.getY();

	    if(checkP3Dist){
	    p3dx = point3[1].x - point3[0].x;
	    p3dy = point3[1].y - point3[0].y;
	    }
	    Vector3D vPanDir = vMid.subtract(vPmid);
	    vPanDir = vPanDir.normalize();
	    vPanDir = vPanDir.scalarMultiply((double)50);

	    //calculate delta
	    p1delta = sqrt(p1dx * p1dx + p1dy * p1dy);
	    p2delta = sqrt(p2dx * p2dx + p2dy * p2dy);
	    p3delta = sqrt(p3dx * p3dx + p3dy * p3dy);
	    
	    //calculate distance between 2 points
	    p1Dist = point1[1].dist(point1[0]);
	    p2Dist = point2[1].dist(point2[0]);
	    p3Dist = point3[1].dist(point3[0]);
	    
	    //calculate delta between 2 points
	    curDist = point1[1].dist(point2[1]);
	    prevDist = point2[0].dist(point1[0]);
	    compDist = curDist - prevDist;
	
	    }
	}
	
	void drawPoints(){
		  
//		   fill(0,50,0);
//		   noStroke();
//		   for (Iterator itr=spots.iterator(); itr.hasNext();) {
//		    Loc p=(Loc) itr.next();
//		    ellipse(p.x*width, p.y*height, 3, 3);
//		   }
//		
//		  fill(50,0,0);
//		  noStroke();
//		  for (Iterator itr=spots2.iterator(); itr.hasNext();) { 
//			 Loc p2=(Loc) itr.next();
//			 ellipse(p2.x*width, p2.y*height, 3, 3);
//	     	}
//		
		  stroke(255);
		  fill(210,30);
		  
		  //draw points
		  ellipse(point1[1].x,point1[1].y,10,10);
		  ellipse(point2[1].x,point2[1].y,10,10);
		  
		  //draw line between 2 points
		  line(point1[1].x,point1[1].y,point2[1].x,point2[1].y);
		  
		  //draw midpoint
		  ellipse(point3[1].x , point3[1].y,10,10);
		  
		  //trails
		  stroke(210,30);
		  ellipse(point1[0].x,point1[0].y,10,10);
		  ellipse(point2[0].x,point2[0].y,10,10);
		  
		  //draw line between 2 points
		  line(point1[0].x,point1[0].y,point2[0].x,point2[0].y);
		  
		  //draw midpoint
		  ellipse(point3[0].x , point3[0].y,10,10);
		  fill(255);
		  stroke(255);
		  //draw distance / delta checker
		  strokeWeight(3);
		  text("CurDist "+curDist,10,10);
		  text("compDist "+compDist,10,25);
		  text("p3Dist "+p3Dist,10,40);
		  line((width/2)-(curDist/2),10,(width/2)+(curDist/2),10);
		  
		  line((width/2),20,(width/2)+compDist*2,20);
		  
		  line((width/2),30,(width/2)+p3delta*2,30);
		  //line(30,50,10+p3delta,50);
		  strokeWeight(1);
		  
		}

	public void keyPressed(){

	  if(key == 't' || key == 'T'){

	    if(varText){
	      varText = false;
	    }
	    else {
	      varText = true;
	    }
	  }
		  if(key == 'e' || key == 'E'){

			    if(drawSpace){
			      drawSpace = false;
			    }
			    else {
			      drawSpace = true;
			    }
	  }
	     if(key == 's' || key == 'S'){

	    if(irSensorTest){
	      irSensorTest = false;
	    }
	    else {
	      irSensorTest = true;
	    }

	  } 
	  if(key == 'd' || key == 'D'){

	    if(ir.debug){
	      ir.debug = false;
	    }
	    else {
	      ir.debug = true;
	    }  
	  }
	    if(key == 'r' || key == 'R'){

	      cam.reset();

	   }       
	}
	
	//T : TEXT
	//E : DRAW MODEL
	//S : SENSOR TEST
	//D : DEBUG
	//R : RESET
	void checkVariables(){
		fill(255);
	  text("debug : "+ ir.debug, 10, textStart + lineHeight);
	  text("irSensorTest : " + irSensorTest,10,textStart + lineHeight*2);
	 // text("connectiong : "+ ir.isCalibrating(), 10, textStart + lineHeight*3);

	  
	  
	  text("point1[1] : "+ point1[1], 10, textStart + lineHeight*4);
	  text("point1[0] : "+ point1[0], 10, textStart + lineHeight*5);
	  text("point2[1]  : " + point2[1], 10, textStart + lineHeight*6);
	  text("point2[0]  : "+  point2[0] , 10, textStart + lineHeight*7);
	  
	  text("point3[1]  : "+ point3[1] , 10, textStart + lineHeight*8);
	  text("point3[0]  : "+ point3[0] , 10, textStart + lineHeight*9);
	 

	  text("pen(0) : "+ ir.pen(0), 10, textStart + lineHeight*10);
	  text("pen(1) : "+ ir.pen(1), 10, textStart + lineHeight*11);
	  
	  text("npen() : "+ ir.npen(), 10, textStart + lineHeight *12);
	  text("p1Dist : "+ checkP1Dist, 10, textStart + lineHeight *13);	  
	  text("p2Dist : "+ checkP2Dist, 10, textStart + lineHeight *14);
	}
	
	public static void main(String args[])
	{
	  PApplet.main(new String[] { dev.Robo_Sketch.class.getName() });
	}

	//USE FOR FULL SCREEN MODE
	//public static void main(String args[])
	//{
//	  PApplet.main(new String[] { "--present", dev.Robo_Sketch.class.getName() });
	//}
}


